#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/time.h>
#include <sys/mman.h>

#define SLOG(level,fmt,args...) printf(fmt,##args)

static void *pinmux_vaddr;
static void *s_trig_vaddr;
static void *s_echo_vaddr;
static int mem_fd;


#define GPIO_DIR_REG 0x400
#define GPIO_DATA_REG 0x3FC // 0x000 - 0x3FC

/* Hi3516EV100
 *
 * Trig - GPIO3_3
 * Echo - GPIO3_1
 */
#define PINMUX_BASE_ADDR (0x12040000)
#define GPIO3_BASE_ADDR (0x12143000)

//#define TRIG_PINMUX_ADDR 0x12040088
//#define ECHO_PINMUX_ADDR 0x12040090
#define TRIG_PINMUX_OFFSET 0x88
#define ECHO_PINMUX_OFFSET 0x90

#define TRIG_PINMUX_VAL 0x0
#define ECHO_PINMUX_VAL 0x0

#define TRIG_GPIO_ADDR GPIO3_BASE_ADDR
#define ECHO_GPIO_ADDR GPIO3_BASE_ADDR

#define TRIG_GPIO_PIN 0x3
#define ECHO_GPIO_PIN 0x1
#define TRIG_DATA_REG (0x1<<(TRIG_GPIO_PIN+2))
#define ECHO_DATA_REG (0x1<<(ECHO_GPIO_PIN+2))

inline void sleep_us_bycpu(int us)
{
    // count 0 -> 1000 cost ~20us
    size_t count = us * 50;
    size_t i = 0;
    while(i<=count) i += 1;
}
/**** End Hi3516EV100 ***/

int init_trig_echo_pins(void)
{
    unsigned char reg;
    mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (mem_fd <= 0)
    {
	SLOG(ERR, "open mem fd err!\n");
	return -1;
    }
    pinmux_vaddr = mmap(0, 4096, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, PINMUX_BASE_ADDR);
    s_trig_vaddr = mmap(0, 4096, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, TRIG_GPIO_ADDR);
    if(ECHO_GPIO_ADDR != TRIG_GPIO_ADDR) {
	s_echo_vaddr = mmap(0, 4096, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, ECHO_GPIO_ADDR);
    } else {
	s_echo_vaddr = s_trig_vaddr;
    }

    if (pinmux_vaddr == NULL || s_trig_vaddr == NULL || s_echo_vaddr == NULL)
    {
	SLOG(ERR, "mmap trig or echo gpio err!\n");
	return -1;
    }
    SLOG(PRI,"mmap pinmux addr is 0x%x, s_trig_vaddr 0x%x, s_echo_vaddr 0x%x\n",pinmux_vaddr, s_trig_vaddr,s_echo_vaddr);

    //pinmux setting
    *(volatile unsigned char *)(pinmux_vaddr + TRIG_PINMUX_OFFSET) = TRIG_PINMUX_VAL;
    usleep(100000);
    *(volatile unsigned char *)(pinmux_vaddr + ECHO_PINMUX_OFFSET) = ECHO_PINMUX_VAL;

    //direction
    reg = *(volatile unsigned char *)(s_trig_vaddr + GPIO_DIR_REG);
    reg |= (0x1<<TRIG_GPIO_PIN); // 1 - output
    *(volatile unsigned char *)(s_trig_vaddr + GPIO_DIR_REG) = reg;
    usleep(100000);
    //Trig inital low level
    *(volatile unsigned char *)(s_trig_vaddr + TRIG_DATA_REG) = 0x0;

    reg = *(volatile unsigned char *)(s_echo_vaddr + GPIO_DIR_REG);
    reg &= ~(0x1<<ECHO_GPIO_PIN); // 0 - input
    *(volatile unsigned char *)(s_echo_vaddr + GPIO_DIR_REG) = reg;
    usleep(100000);

    return 0;
}

void exit_trig_echo_pins(void)
{
    SLOG(PRI, "exit ...!\n");
    munmap(pinmux_vaddr, 4096);
    munmap(s_trig_vaddr, 4096);
    if(s_echo_vaddr != s_trig_vaddr) {
	munmap(s_echo_vaddr, 4096);
    }
    close(mem_fd);
}

inline void write_trig(int us)
{
    *(volatile unsigned char *)(s_trig_vaddr + TRIG_DATA_REG) = (0x1<<TRIG_GPIO_PIN);
    sleep_us_bycpu(us);
    *(volatile unsigned char *)(s_trig_vaddr + TRIG_DATA_REG) = 0x0;
}

inline int read_echo(void)
{
    // wait echo level to high
    while(0x0 == *(volatile unsigned char *)(s_echo_vaddr + ECHO_DATA_REG))
    {
        sleep_us_bycpu(10);
    }

    int tus = 0;
    // calc echo high level timelength
    while(0x0 != *(volatile unsigned char *)(s_echo_vaddr + ECHO_DATA_REG))
    {
	sleep_us_bycpu(10);
	tus += 10;
    }
    return tus;
}

int main(void)
{
    init_trig_echo_pins();

    if (s_trig_vaddr == NULL || s_echo_vaddr == NULL)
    {
	SLOG(ERR, "init gpio fail\n");
	return -1;
    }

    int duration_us = 0;
    int trig_us = 100;
    while(1)
    {
	printf("Trig High %d us...\n",trig_us);
	write_trig(trig_us);
	printf("Echo Wait ...\n");
	duration_us = read_echo();
	printf("Echo Duration: %d us, Distance : %d cm\n",duration_us, duration_us/58);

	sleep(5);
    }

    exit_trig_echo_pins();
    return 0;
}

